#include "stdafx.h"
#include "gp_XYZ.h"
#include "gp_Trsf.h"
#include "gp_GTrsf.h"
#include "gp_Mat.h"
#include "gp.h"

#define Mat00 matrix[0][0]
#define Mat01 matrix[0][1]
#define Mat02 matrix[0][2]
#define Mat10 matrix[1][0]
#define Mat11 matrix[1][1]
#define Mat12 matrix[1][2]
#define Mat20 matrix[2][0]
#define Mat21 matrix[2][1]
#define Mat22 matrix[2][2]

#define Nat00 NewMat.matrix[0][0]
#define Nat01 NewMat.matrix[0][1]
#define Nat02 NewMat.matrix[0][2]
#define Nat10 NewMat.matrix[1][0]
#define Nat11 NewMat.matrix[1][1]
#define Nat12 NewMat.matrix[1][2]
#define Nat20 NewMat.matrix[2][0]
#define Nat21 NewMat.matrix[2][1]
#define Nat22 NewMat.matrix[2][2]

#define Oat00 Other.matrix[0][0]
#define Oat01 Other.matrix[0][1]
#define Oat02 Other.matrix[0][2]
#define Oat10 Other.matrix[1][0]
#define Oat11 Other.matrix[1][1]
#define Oat12 Other.matrix[1][2]
#define Oat20 Other.matrix[2][0]
#define Oat21 Other.matrix[2][1]
#define Oat22 Other.matrix[2][2]

gp_Mat::gp_Mat () {
	Mat00 =
		Mat01 =
		Mat02 =
		Mat10 =
		Mat11 =
		Mat12 =
		Mat20 =
		Mat21 =
		Mat22 = 0.0;
}

gp_Mat::gp_Mat (const double a11,
	const double a12,
	const double a13,
	const double a21,
	const double a22,
	const double a23,
	const double a31,
	const double a32,
	const double a33) {

		Mat00 = a11;
		Mat01 = a12;
		Mat02 = a13;
		Mat10 = a21;
		Mat11 = a22;
		Mat12 = a23;
		Mat20 = a31;
		Mat21 = a32;
		Mat22 = a33;
}

void gp_Mat::SetDiagonal (const double X1,
	const double X2,
	const double X3)
{
	Mat00 = X1;   Mat11 = X2;   Mat22 = X3;
}

void gp_Mat::SetIdentity ()
{
	Mat00 = Mat11 = Mat22 = 1.0;
	Mat01 = Mat02 = Mat10  = Mat12 = Mat20 = Mat21 = 0.0;
}

void gp_Mat::SetScale (const double S)
{
	Mat00 = Mat11 =  Mat22 = S;
	Mat01 = Mat02 = Mat10 = Mat12 = Mat20 = Mat21 = 0.0;
}

void gp_Mat::SetValue (const int Row, 
	const int Col, 
	const double Value)
{
	Standard_OutOfRange_Raise_if
		(Row < 1 || Row > 3 || Col < 1 || Col > 3, " ");
	matrix[Row-1][Col-1] = Value;
}

double gp_Mat::Determinant () const
{
	return
		Mat00 * (Mat11 * Mat22 - Mat21 * Mat12) -
		Mat01 * (Mat10 * Mat22 - Mat20 * Mat12) +
		Mat02 * (Mat10 * Mat21 - Mat20 * Mat11);
}

const double& gp_Mat::Value (const int Row, 
	const int Col) const
{
	Standard_OutOfRange_Raise_if
		(Row < 1 || Row > 3 || Col < 1 || Col > 3, " ");
	return matrix[Row-1][Col-1];
}

double& gp_Mat::ChangeValue (const int Row, 
	const int Col)
{
	Standard_OutOfRange_Raise_if
		(Row < 1 || Row > 3 || Col < 1 || Col > 3, " ");
	return matrix[Row-1][Col-1];
}

bool gp_Mat::IsSingular () const
{
	// Pour etre sur que Gauss va fonctionner, il faut faire Gauss ...
	double val = Determinant();
	if (val < 0) val = - val;
	return val <= gp::Resolution();
}

void gp_Mat::Add (const gp_Mat& Other)
{
	Mat00 = Mat00 + Oat00;
	Mat01 = Mat01 + Oat01;
	Mat02 = Mat02 + Oat02;
	Mat10 = Mat10 + Oat10;
	Mat11 = Mat11 + Oat11;
	Mat12 = Mat12 + Oat12;
	Mat20 = Mat20 + Oat20;
	Mat21 = Mat21 + Oat21;
	Mat22 = Mat22 + Oat22;
}

gp_Mat gp_Mat::Added (const gp_Mat& Other) const
{
	gp_Mat NewMat;
	Nat00 = Mat00 + Oat00;
	Nat01 = Mat01 + Oat01;
	Nat02 = Mat02 + Oat02;
	Nat10 = Mat10 + Oat10;
	Nat11 = Mat11 + Oat11;
	Nat12 = Mat12 + Oat12;
	Nat20 = Mat20 + Oat20;
	Nat21 = Mat21 + Oat21;
	Nat22 = Mat22 + Oat22;
	return NewMat;
}

void gp_Mat::Divide (const double Scalar)
{
	double val = Scalar;
	if (val < 0) val = - val;
	Standard_ConstructionError_Raise_if
		(val <= gp::Resolution(),"gp_Mat : Divide by 0");
	double UnSurScalar = 1.0 / Scalar;
	Mat00 *= UnSurScalar;
	Mat01 *= UnSurScalar; 
	Mat02 *= UnSurScalar; 
	Mat10 *= UnSurScalar; 
	Mat11 *= UnSurScalar; 
	Mat12 *= UnSurScalar; 
	Mat20 *= UnSurScalar; 
	Mat21 *= UnSurScalar; 
	Mat22 *= UnSurScalar; 
}

gp_Mat gp_Mat::Divided (const double Scalar) const
{
	double val = Scalar;
	if (val < 0) val = - val;
	Standard_ConstructionError_Raise_if
		(val <= gp::Resolution(),"gp_Mat : Divide by 0");
	gp_Mat NewMat;
	double UnSurScalar = 1.0 / Scalar;
	Nat00 = Mat00 * UnSurScalar;
	Nat01 = Mat01 * UnSurScalar; 
	Nat02 = Mat02 * UnSurScalar; 
	Nat10 = Mat10 * UnSurScalar; 
	Nat11 = Mat11 * UnSurScalar; 
	Nat12 = Mat12 * UnSurScalar; 
	Nat20 = Mat20 * UnSurScalar; 
	Nat21 = Mat21 * UnSurScalar; 
	Nat22 = Mat22 * UnSurScalar; 
	return NewMat;
}

gp_Mat gp_Mat::Multiplied (const gp_Mat& Other) const
{
	gp_Mat NewMat = *this;
	NewMat.Multiply(Other);
	return NewMat;
}

void gp_Mat::Multiply (const gp_Mat& Other)
{
	double T00,T01,T02,T10,T11,T12,T20,T21,T22;
	T00 = Mat00 * Oat00 + Mat01 * Oat10 + Mat02 * Oat20;
	T01 = Mat00 * Oat01 + Mat01 * Oat11 + Mat02 * Oat21;
	T02 = Mat00 * Oat02 + Mat01 * Oat12 + Mat02 * Oat22;
	T10 = Mat10 * Oat00 + Mat11 * Oat10 + Mat12 * Oat20;
	T11 = Mat10 * Oat01 + Mat11 * Oat11 + Mat12 * Oat21;
	T12 = Mat10 * Oat02 + Mat11 * Oat12 + Mat12 * Oat22;
	T20 = Mat20 * Oat00 + Mat21 * Oat10 + Mat22 * Oat20;
	T21 = Mat20 * Oat01 + Mat21 * Oat11 + Mat22 * Oat21;
	T22 = Mat20 * Oat02 + Mat21 * Oat12 + Mat22 * Oat22;
	Mat00 = T00; 
	Mat01 = T01; 
	Mat02 = T02; 
	Mat10 = T10; 
	Mat11 = T11; 
	Mat12 = T12; 
	Mat20 = T20; 
	Mat21 = T21; 
	Mat22 = T22; 
}

void gp_Mat::PreMultiply (const gp_Mat& Other)
{
	double T00,T01,T02,T10,T11,T12,T20,T21,T22;
	T00 = Oat00 * Mat00 + Oat01 * Mat10 + Oat02 * Mat20;
	T01 = Oat00 * Mat01 + Oat01 * Mat11 + Oat02 * Mat21;
	T02 = Oat00 * Mat02 + Oat01 * Mat12 + Oat02 * Mat22;    
	T10 = Oat10 * Mat00 + Oat11 * Mat10 + Oat12 * Mat20;
	T11 = Oat10 * Mat01 + Oat11 * Mat11 + Oat12 * Mat21;
	T12 = Oat10 * Mat02 + Oat11 * Mat12 + Oat12 * Mat22;    
	T20 = Oat20 * Mat00 + Oat21 * Mat10 + Oat22 * Mat20;
	T21 = Oat20 * Mat01 + Oat21 * Mat11 + Oat22 * Mat21;
	T22 = Oat20 * Mat02 + Oat21 * Mat12 + Oat22 * Mat22;    
	Mat00 = T00; 
	Mat01 = T01; 
	Mat02 = T02; 
	Mat10 = T10; 
	Mat11 = T11; 
	Mat12 = T12; 
	Mat20 = T20; 
	Mat21 = T21; 
	Mat22 = T22; 
}

gp_Mat gp_Mat::Multiplied (const double Scalar) const
{
	gp_Mat NewMat;
	Nat00 = Scalar * Mat00;
	Nat01 = Scalar * Mat01;
	Nat02 = Scalar * Mat02;
	Nat10 = Scalar * Mat10;
	Nat11 = Scalar * Mat11;
	Nat12 = Scalar * Mat12;
	Nat20 = Scalar * Mat20;
	Nat21 = Scalar * Mat21;
	Nat22 = Scalar * Mat22;
	return NewMat;
}

void gp_Mat::Multiply (const double Scalar)
{
	Mat00 *= Scalar; 
	Mat01 *= Scalar; 
	Mat02 *= Scalar; 
	Mat10 *= Scalar; 
	Mat11 *= Scalar; 
	Mat12 *= Scalar; 
	Mat20 *= Scalar; 
	Mat21 *= Scalar; 
	Mat22 *= Scalar; 
}

gp_Mat gp_Mat::Powered (const int N) const
{
	gp_Mat MatN = *this;
	MatN.Power (N);
	return MatN;
}

void gp_Mat::Subtract (const gp_Mat& Other)
{
	Mat00 -= Oat00;
	Mat01 -= Oat01;
	Mat02 -= Oat02;
	Mat10 -= Oat10;
	Mat11 -= Oat11;
	Mat12 -= Oat12;
	Mat20 -= Oat20;
	Mat21 -= Oat21;
	Mat22 -= Oat22;
}

gp_Mat gp_Mat::Subtracted (const gp_Mat& Other) const
{
	gp_Mat NewMat;
	Nat00 = Mat00 - Oat00;
	Nat01 = Mat01 - Oat01;
	Nat02 = Mat02 - Oat02;
	Nat10 = Mat10 - Oat10;
	Nat11 = Mat11 - Oat11;
	Nat12 = Mat12 - Oat12;
	Nat20 = Mat20 - Oat20;
	Nat21 = Mat21 - Oat21;
	Nat22 = Mat22 - Oat22;
	return NewMat;
}

void gp_Mat::Transpose ()
{
	double Temp;
	Temp   = Mat01;
	Mat01  = Mat10;
	Mat10  = Temp;
	Temp   = Mat02;
	Mat02  = Mat20;
	Mat20  = Temp;
	Temp   = Mat12;
	Mat12  = Mat21;
	Mat21  = Temp;
}

gp_Mat gp_Mat::Transposed () const
{
	gp_Mat NewMat = *this;
	NewMat.Transpose();
	return NewMat; 
}

gp_Mat operator* (const double Scalar, const gp_Mat& Mat3D)
{ return Mat3D.Multiplied (Scalar); }


gp_Mat::gp_Mat (const gp_XYZ& Col1,
		const gp_XYZ& Col2,
		const gp_XYZ& Col3)
{
  Mat00 = Col1.X(); Mat10 = Col1.Y(); Mat20 = Col1.Z();
  Mat01 = Col2.X(); Mat11 = Col2.Y(); Mat21 = Col2.Z();
  Mat02 = Col3.X(); Mat12 = Col3.Y(); Mat22 = Col3.Z();
}

void gp_Mat::SetCol (const int Col,
		     const gp_XYZ& Value) {

  Standard_OutOfRange_Raise_if (Col < 1 || Col > 3, " ");
  if      (Col == 1) {
    Mat00 = Value.X(); Mat10 = Value.Y(); Mat20 = Value.Z();
  }
  else if (Col == 2) {
    Mat01 = Value.X(); Mat11 = Value.Y(); Mat21 = Value.Z();
  }
  else {
    Mat02 = Value.X(); Mat12 = Value.Y(); Mat22 = Value.Z();
  }
}

void gp_Mat::SetCols (const gp_XYZ& Col1,
		      const gp_XYZ& Col2,
		      const gp_XYZ& Col3)
{
  Mat00 = Col1.X(); Mat10 = Col1.Y(); Mat20 = Col1.Z();
  Mat01 = Col2.X(); Mat11 = Col2.Y(); Mat21 = Col2.Z();
  Mat02 = Col3.X(); Mat12 = Col3.Y(); Mat22 = Col3.Z();
}

void gp_Mat::SetCross (const gp_XYZ& Ref)
{
  double X = Ref.X();
  double Y = Ref.Y();
  double Z = Ref.Z();
  Mat00 = Mat11 = Mat22 = 0.0;
  Mat01 = - Z;  
  Mat02 = Y;
  Mat12 = - X;
  Mat10 = Z;
  Mat20 = - Y;
  Mat21 = X;
}

void gp_Mat::SetDot (const gp_XYZ& Ref)
{
  double X = Ref.X();
  double Y = Ref.Y();
  double Z = Ref.Z();
  Mat00 = X * X;
  Mat11 = Y * Y;
  Mat22 = Z * Z;
  Mat01 = X * Y;
  Mat02 = X * Z;
  Mat12 = Y * Z;
  Mat10 = Mat01;
  Mat20 = Mat02;
  Mat21 = Mat12;
}

void gp_Mat::SetRotation (const gp_XYZ& Axis,
			  const double Ang)
{
  //    Rot = I + sin(Ang) * M + (1. - cos(Ang)) * M*M
  //    avec  M . XYZ = Axis ^ XYZ

//  const void* M = (void*)&(matrix[0][0]);
  gp_XYZ V = Axis.Normalized();
  SetCross (V);
  Multiply (sin(Ang));
  gp_Mat Temp;
  Temp.SetScale (1.0);
  Add (Temp);
  double A = V.X();
  double B = V.Y();
  double C = V.Z();
  Temp.SetRow (1, gp_XYZ(- C*C - B*B,      A*B,           A*C     ));
  Temp.SetRow (2, gp_XYZ(     A*B,      -A*A - C*C,        B*C    ));
  Temp.SetRow (3, gp_XYZ(     A*C,          B*C,       - A*A - B*B));
  Temp.Multiply (1.0 - cos(Ang));
  Add (Temp);
}

void gp_Mat::SetRow (const int Row,
		     const gp_XYZ& Value)
{
  Standard_OutOfRange_Raise_if (Row < 1 || Row > 3, " ");
  if      (Row == 1) {
    Mat00 = Value.X(); Mat01 = Value.Y(); Mat02 = Value.Z();
  }
  else if (Row == 2) {
    Mat10 = Value.X(); Mat11 = Value.Y(); Mat12 = Value.Z();
  }
  else {
    Mat20 = Value.X(); Mat21 = Value.Y(); Mat22 = Value.Z();
  }
}

void gp_Mat::SetRows (const gp_XYZ& Row1,
		      const gp_XYZ& Row2,
		      const gp_XYZ& Row3)
{
  Mat00 = Row1.X(); Mat01 = Row1.Y(); Mat02 = Row1.Z();
  Mat10 = Row2.X(); Mat11 = Row2.Y(); Mat12 = Row2.Z();
  Mat20 = Row3.X(); Mat21 = Row3.Y(); Mat22 = Row3.Z();
}

gp_XYZ gp_Mat::Column (const int Col) const
{
  Standard_OutOfRange_Raise_if (Col < 1 || Col > 3, "");
  if (Col == 1) return gp_XYZ (Mat00,Mat10,Mat20);
  if (Col == 2) return gp_XYZ (Mat01,Mat11,Mat21);
  return gp_XYZ (Mat02,Mat12,Mat22);
}

gp_XYZ gp_Mat::Diagonal () const
{
  return gp_XYZ (Mat00, Mat11, Mat22);
}

gp_XYZ gp_Mat::Row (const int Row) const
{
  Standard_OutOfRange_Raise_if (Row < 1 || Row > 3, "");
  if (Row == 1) return gp_XYZ (Mat00,Mat01,Mat02);
  if (Row == 2) return gp_XYZ (Mat10,Mat11,Mat12);
  return gp_XYZ (Mat20,Mat21,Mat22);
}

void gp_Mat::Invert ()
{ 
  gp_Mat NewMat;    
  //
  // calcul de  la transposee de la commatrice
  //
  Nat00 = Mat11 * Mat22 - Mat12 * Mat21 ;
  Nat10 = -(Mat10 * Mat22 - Mat20 * Mat12) ;
  Nat20 = Mat10 * Mat21 - Mat20 * Mat11 ;
  Nat01 = - (Mat01 * Mat22 - Mat21 * Mat02) ;
  Nat11 = Mat00 * Mat22 - Mat20 * Mat02 ;
  Nat21 = -(Mat00 * Mat21 - Mat20 * Mat01) ;
  Nat02 = Mat01 * Mat12 - Mat11 * Mat02 ;
  Nat12 = -(Mat00 * Mat12 - Mat10 * Mat02) ;
  Nat22 = Mat00 * Mat11 - Mat01 * Mat10 ;
  double det =  Mat00 * Nat00 + Mat01* Nat10 + Mat02 * Nat20 ;
  double val = det;
  if (val < 0) val = - val;
  Standard_ConstructionError_Raise_if
    (val <= gp::Resolution(),"");
  det = 1.0e0 / det ;
  Mat00 = Nat00;
  Mat10 = Nat10;
  Mat20 = Nat20;
  Mat01 = Nat01;
  Mat11 = Nat11;
  Mat21 = Nat21;
  Mat02 = Nat02;
  Mat12 = Nat12;
  Mat22 = Nat22;
  Multiply(det) ;
}

gp_Mat gp_Mat::Inverted () const
{ 
  gp_Mat NewMat;
  //
  // calcul de  la transposee de la commatrice
  //
  Nat00 = Mat11 * Mat22 - Mat12 * Mat21 ;
  Nat10 = -(Mat10 * Mat22 - Mat20 * Mat12) ;
  Nat20 = Mat10 * Mat21 - Mat20 * Mat11 ;
  Nat01 = - (Mat01 * Mat22 - Mat21 * Mat02) ;
  Nat11 = Mat00 * Mat22 - Mat20 * Mat02 ;
  Nat21 = -(Mat00 * Mat21 - Mat20 * Mat01) ;
  Nat02 = Mat01 * Mat12 - Mat11 * Mat02 ;
  Nat12 = -(Mat00 * Mat12 - Mat10 * Mat02) ;
  Nat22 = Mat00 * Mat11 - Mat01 * Mat10 ;
  double det =  Mat00 * Nat00 + Mat01* Nat10 + Mat02 * Nat20 ;
  double val = det;
  if (val < 0) val = - val;
  Standard_ConstructionError_Raise_if
    (val <= gp::Resolution(),"");
  det = 1.0e0 / det ;
  NewMat.Multiply(det) ;
  return NewMat;
}

void gp_Mat::Power (const int N)
{
  if (N == 1) { }
  else if (N == 0)  { SetIdentity() ; }
  else if (N == -1) { Invert(); }
  else {
    if (N < 0) { Invert(); }
    int Npower = N;
    if (Npower < 0) Npower = - Npower;
    Npower--;
    gp_Mat Temp = *this;
    for(;;) {
      if (IsOdd(Npower)) Multiply (Temp);
      if (Npower == 1)   break; 
      Temp.Multiply (Temp);
      Npower>>=1;
    }
  }
}

